#include <stdio.h>
#include <math.h>
#include "p5glove.h"


#include <iostream>
#include <string.h>

#include "OscOutboundPacketStream.h"

#include "ip/NetworkingUtils.h"

#include "ip/UdpSocket.h"
#include "ip/IpEndpointName.h"

#define IP_MTU_SIZE 1536


int main (int argc, const char * argv[]) {
   
	P5Glove theGlove;
	struct p5glove_data theData;
	
	int visibleLEDs[8];

	// Open the Glove
	
	theGlove = p5glove_open();
	
	
	if(theGlove != NULL)
		printf("\n Glove Connect Succeeded\n");
	else
	{
		printf("Glove Connect Failed");
		return 0;
	}
		

	// Initialise the send address

    char *hostName = "localhost";
    int port = 7000;
    
	// Parse command line for host and port
	

    if( argc == 3 )
	{
		if( strcmp(argv[1],  "-h") == 0)
			hostName = (char *) argv[2];
			
		
		if( strcmp(argv[1],  "-p") == 0)
				port = atoi( argv[2] );
		
	}
	
	if( argc == 5 )
	{
		if(strcmp(argv[1], "-h")==0)
				hostName = (char *) argv[2];
				
		if(strcmp(argv[3], "-h")==0)
				hostName = (char *) argv[4];
		
		if(strcmp( argv[1], "-p")==0)
				port = atoi( argv[2] );
				
		if(strcmp(argv[3], "-p")==0)
				port = atoi( argv[4] );
		
	}
	
	bool displayData = false;
	
	for(int i = 0; i<argc; i++)
	{
		if(strcmp(argv[i], "-d")==0)
				 displayData = true;

	}

	printf(" Host = %s, Port = %d \n",  hostName, port, displayData);
	
	// Set up socket address
	IpEndpointName host( hostName, port );

	char hostIpAddress[ IpEndpointName::ADDRESS_STRING_LENGTH ];
	host.AddressAsString( hostIpAddress );

		
	char buffer[IP_MTU_SIZE];
    osc::OutboundPacketStream p( buffer, IP_MTU_SIZE );
	UdpTransmitSocket socket( host );

	// Main Loop 
	
	while(1)
	{
		int visibleCount = 0;
		int x = 0;
		int y = 0;
		int z = 0;
		
		//  Sample the glove
		p5glove_sample(theGlove, &theData);
		p5glove_process_sample(theGlove, &theData);
		
		// Establish which LEDs or visible
		for(int i= 0; i<8;i++)
		{
			if(theData.ir[i].visible)
			{
				visibleLEDs[visibleCount] = i;
				visibleCount++;
				
				x += theData.ir[i].x;
				y += theData.ir[i].y;
				z += theData.ir[i].z;
				
			}
		
		}	
		 
		// Simple Averaging of visible leds gives a centroid of sorts
				
		if(visibleCount>0)
		{
			// If there are visisble LEDs,  average LEDS 
			x = x/visibleCount;
			y = y/visibleCount;
			z = z/visibleCount;
			
			p.Clear();
			
			// Send data with  X Y Z

			p << osc::BeginBundle();
			
				// The name-address
				p << osc::BeginMessage( "/p5glove_data" )
					// The Buttons
					<< (theData.buttons & 1) << ((theData.buttons&2)>>1 ) << ((theData.buttons&4)>>2) 
					// The Fingers
					<< theData.finger[4] << theData.finger[0] << theData.finger[1] << theData.finger[2] << theData.finger[3] 
					// X Y Z
					<< x << y << z
				<<  osc::EndMessage;
				
			p << osc::EndBundle;
	
			socket.Send( p.Data(), p.Size() );
			
			if(displayData)
				printf(" Buttons: %d %d %d Fingers: %d %d %d %d %d Coords: %d %d %d \n",  
					(theData.buttons & 1), ((theData.buttons&2)>>1 ), ((theData.buttons&4)>>2),
					theData.finger[4], theData.finger[0], theData.finger[1], theData.finger[2],  theData.finger[3],
					x,  y, z);
					
		}
		else
		{
		
		// Otherwise dont send X Y Z
		// This solution is better for most apps
		
			p.Clear();
			
			p << osc::BeginBundle();
			
				// The name-address
				p << osc::BeginMessage( "/p5glove_data" )
					// The Buttons
					<< (theData.buttons & 1) << ((theData.buttons&2)>>1 ) << ((theData.buttons&4)>>2)
					// The Fingers
					<< theData.finger[4] << theData.finger[0] << theData.finger[1] << theData.finger[2] << theData.finger[3] 
				<<  osc::EndMessage;
					  
			p << osc::EndBundle;
	
			socket.Send( p.Data(), p.Size() );
			
			if(displayData)
				printf(" Buttons: %d %d %d Fingers: %d %d %d %d %d \n",  
					(theData.buttons & 1), ((theData.buttons&2)>>1 ), ((theData.buttons&4)>>2),
					theData.finger[4], theData.finger[0], theData.finger[1], theData.finger[2],  theData.finger[3]);
		
		}
		
		
	}
	
    return 0;
}
